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Abstract 

N onholonormc  mechanical  systems  are  governed  by  constraints  of  motion  that  are  nonintegrable 
differential  expressions.  Unlike  holonomic  constraints,  these  differential  constraints  do  not  reduce  tfie 
number  of  dimensions  of  tfie  configuration  space  of  a  system.  Therefore  a  nonholonomic  system  can 
access  a  configuration  space  of  dimension  higher  than  the  number  of  t)ie  degrees  of  freedom  of  the 
system.  In  this  paper,  we  develop  an  algorithm  for  planning  admissible  trajectories  for  nonholonomic 
systems  that  will  take  the  system  from  one  point  in  its  configuration  space  to  another.  In  our  algorithm 
we  first  converge  the  independent  variables  to  their  desired  values  and  Uien  use  closed  trajectories  of  the 
independent  variable  to  converge  the  dependent  variables.  We  use  Stokes's  theorem  in  our  algorithm 
to  convert  the  problem  of  finding  a  closed  path  into  that  of  finding  a  surface  area  in  the  space  of 
the  independent  variables,  such  that  the  dependent  variables  converge  to  their  desired  values  as  the 
independent  variables  traverse  along  the  boundary  of  this  surface  area.  The  use  of  Stokes's  theorem 
simplifies  the  motion  planning  problem  and  also  imparts  global  characteristics.  The  salient  features  of 
our  algorithm  are  apparent  in  the  two  examples  we  discuss  -  a  planar  space  robot  and  a  disk  rolling 
without  slipping  on  a  flat  surface. 
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1.   Introduction 

Nonholonomic  meclianical  systems  are  governed  by  constraints  of  motion  that  are 
nonintegrable  differential  expressions  of  the  form 


Y^  aj.  d(ii  +  ajt  dt  =  0,         j  =  l,2,-—m  (1) 

i=i 

where,  the  <?'s  represent  the  generalized  coordinates,  t  represents  time,  and  the  a's  are, 
in  general  functions  of  the  q's  and  t.  As  a  result  of  the  nonintegrable  nature  of  these 
differential  constraints,  it  is  not  possible  to  obtain  functions  of  the  form 

0i(gi.02»-  •,qn,t)  =  o,         j  =  l,2,--m  (2) 

that  will  enable  us  to  eliminate  some  of  the  dependent  variables.  Naturally,  nonholonomic 
systems  require  more  coordinates  for  their  description  than  there  are  degrees  of  freedom 
in  the  system. 

An  interesting  feature  of  nonholonomic  mechanical  systems  is  their  ability  to  access 
a  configuration  space  of  dimension  higher  than  the  number  of  it's  degrees  of  freedom.  A 
simple  example  is  that  of  a  disk  rolling  without  slipping  on  a  flat  surface.  The  configu- 
ration space  of  the  disk  rolling  on  the  x-y  plane,  shown  in  Fig.l,  is  described  by  the  four 
coordinates  (x,y,0,a),  but  the  degrees  of  freedom  of  the  system  is  only  two  because  of  the 
following  two  nonholonomic  constraints 


dx  -  r  s\n  a  dO  —  0 

dy  -r  cos  a  dO  —  0  (3) 

Inspite  of  having  only  two  degrees  of  freedom,  it  is  quite  intuitive  that  the  rolling  disk  can 
arrive  at  any  configuration  (x,y,0,a)  from  any  other,  through  proper  path  planning.  Such 
a  property  is  common  to  nonholonomic  mechanical  systems  and  can  be  attributed  to  the 
nonintegrable  nature  of  their  differential  constraints. 

For  the  rolling  disk,  our  intuition  can  be  strengthened  if  we  consider  the  following 
example.  Suppose,  it  is  desired  that  the  disk  in  Fig.l  change  its  coordinates  from  (x,y,  6, a) 
to  {xd,yd,6,cx).  Then  a  feasible  trajectory  would  be  the  path  segments  AO  and  OC.  The 
disk  would  roll  forward  from  A  to  O,  and  then  roll  backward  from  O  to  C.  The  individual 
path  segments  AO  and  OC  should  have  equal  lengths  such  that  0  comes  back  to  its  initial 
value  at  the  end  of  the  path.  Furthermore,  the  straight  lines  AB  and  CD  should  be  tangent 
to  the  path  segments  AO  and  OC  respectively,  at  the  points  A  and  C.  This  will  ensure  that 
the  net  change  of  the  variable  a  will  also  be  zero  over  the  complete  path.  Such  a  path  can 
always  be  planned  and  this  leads  us  to  believe  that  the  dependent  variables  x  and  y  can 
indeed  be  changed  arbitrarily  through  cyclic  motion  of  the  independent  variables  0  and  a. 
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Therefore  to  converge  all  the  configuration  variables  of  the  disk  from  one  set  of  values  to 
another,  we  could  first  converge  the  independent  variables  from  their  initial  values  to  their 
desired  values  without  being  concerned  about  the  evolution  of  the  dependent  variables,  and 
then  use  cyclic  motion  of  the  independent  variables  to  converge  the  dependent  variables 
to  their  desired  values. 

In  this  paper  we  will  develop  an  algorithm  for  nonholonomic  motion  planning  -  one 
that  will  enable  us  to  converge  all  the  configuration  variables  of  a  nonholonomic  system 
from  one  set  of  values  to  another.  This  algorithm  will  follow  a  two  step  procedure  -  first 
converge  the  independent  variables,  and  then  use  cyclic  motion  of  the  independent  variables 
to  converge  the  dependent  variables. 

The  nonholonomic  motion  planning  problem  has  been  the  focus  of  attention  of  various 
researchers  in  the  recent  past.  Specifically,  researchers  have  looked  into  the  problems  of 
mobile  robot  navigation  [1],  [5],  [6],  [10],  parking  a  front-wheel  drive  car  or  a  car  with 
multiple  trailers  [9],  [10],  dextrous  manipulation  with  robotic  fingers  [2],  attitude  control 
of  a  satellite  using  two  rotors  instead  of  gas-jets,  reconfiguration  of  a  space  manipulator 
or  a  space  structure  using  only  internal  motion  [12],  etc.  The  multibody  car  system  was 
studied  in  [10]  and  it  was  concluded  that  it  is  a  well  controllable  system.  This  result  was 
obtained  by  first  constructing  the  control  Lie  algebra.  The  controllability  was  concluded 
by  showing  that  the  rank  of  the  control  Lie  algebra  is  equal  to  the  dimension  of  the  state 
space,  at  every  point  in  the  state  space.  Such  an  analysis  only  provides  sufficient  conditions 
for  the  controllability  and  is  useful  for  simple  nonholonomic  systems.  It  cannot  be  used 
to  verify  the  controllability  of  a  complex  system  like  a  6-DOF  space  robot.  Assuming 
the  existence  of  a  feasible  trajectory  connecting  an  initial  and  some  desired  values  of  the 
generalized  coordinates,  the  nonholonomic  motion  planning  of  space  robots  was  discussed 
in  [12].  The  nonholonomy  of  a  space  robot  is  attributed  to  the  conservation  of  its  angular 
momentum.  A  space  robot  consisting  of  a  six  joint  manipulator  mounted  on  a  space 
vehicle  was  described  by  nine  generalized  coordinates  consisting  of  six  joint  angles  of  the 
manipulator  and  the  three  Euler  angles  of  orientation  of  the  space  vehicle.  By  directly 
controlling  only  the  joints  of  the  manipulator,  it  was  shown  that  it  is  possible  to  converge 
all  the  nine  state  variables  to  their  desired  values.  The  trajectory  was  planned  using  a 
Lyapunov  function  and  by  adopting  a  bi-directional  approach. 

An  algorithm  for  steering  a  general  class  of  nonholonomic  systems  was  developed  in 
[11]  using  sinusoids.  This  algorithm  was  applied  for  the  motion  planning  of  a  front  wheel 
drive  cart,  and  reconfiguration  of  a  hopping  robot  in  flight  phase.  In  this  algorithm,  the 
independent  variables  were  first  steered  to  their  desired  configuration  ignoring  the  evolution 
of  the  dependent  variables.  Consequently,  the  dependent  variables  were  converged  to  their 
desired  values  using  closed  trajectories  of  the  independent  variables.  Such  an  approach 
was  proposed  earlier  [15],  for  the  motion  planning  of  a  space  manipulator,  where  the  cyclic 
motion  of  the  joints  of  the  space  manipulator  was  used  to  change  the  orientation  of  the 
whole  system. 


In  this  paper,  we  will  discuss  the  motion  planning  of  nonholonomic  systems  using 
an  algorithm  in  which  the  prerogative  is  to  find  a  closed  trajectory  of  the  independent 
variables  that  converge  the  dependent  variables  to  their  desired  values.  In  our  approach, 
we  use  Stokes's  theorem  to  reduce  this  problem  into  finding  a  surface  area  such  that  the 
dependent  variables  converge  to  their  desired  values  while  the  independent  variables  travel 
along  the  boundary  of  this  surface  area.  The  main  advantage  is  the  global  nature  of  our 
planning  algorithm,  unlike  the  local  path  planning  approach  based  on  Lyapunov  functions 
[12].  Due  to  the  global  nature  of  the  algorithm,  questions  pertaining  to  the  reachability  of 
the  system  can  be  readily  answered,  problems  related  to  singularity  can  be  tackled,  and 
feasible  trajectories  can  be  easily  planned  even  in  the  presence  of  additional  constraints. 
For  a  nonholonomic  system  like  a  space  robot,  these  additional  constraints  may  appear  in 
the  form  of  joint  limits  or  obstacles  in  the  workspace.  Our  algorithm  additionally  provides 
us  with  insight  into  trajectories  that  produce  repeatable  motion.  Repeatability  in  the 
motion  may  be  simply  a  desirable  property  as  in  the  case  of  space  robots,  or  may  even  be 
used  for  singularity  avoidance  as  in  the  case  of  a  rolling  disk. 

This  paper  is  organized  as  follows.  In  section  2  we  discuss  the  mathematical  prelim- 
inaries and  the  properties  of  nonholonomic  systems.  In  section  3,  we  discuss  some  of  the 
issues  related  to  the  motion  planning  of  nonholonomic  systems.  In  section  4,  we  present 
our  algorithm  for  the  nonholonomic  motion  planning  through  examples.  Specifically,  we 
discuss  the  motion  planning  of  a  planar  space  robot  and  a  disk  rolling  without  slipping 
on  a  flat  surface.  The  different  salient  features  of  our  algorithm  are  apparent  in  these  two 
examples. 

2.   Mathematical  preliminaries 

2.1  Line  and  surface  integrals:  Stokes's  theorem 

In  this  section  we  recall  Stokes's  [8]  theorem  used  for  the  tranformation  of  line  integrals 
into  surface  integrals,  and  vice  verca.  The  material  discussed  in  this  section  will  serve  as 
a  mathematical  tool  for  the  trajectory  planning  of  nonholonomic  systems. 

Theorem  1:  Stokes's  Theorem  Let  5  be  a  piecewise  smooth  oriented  surface*  in  space 
and  let  the  boundary  of  S  be  a  piecewise  smooth  closed  curve  C.  Let  v(x,y,z)  be  a 
continuous  vector  function  which  has  continuous  first  partial  derivatives  in  a  domain  in 
space  which  contains  S.  Then 


/  J  nT(V  x  v)dA  =  J>  vids 


(4) 


where,  n  is  the  unit  vector  normal  to  the  surface  S  on  that  side  of  S  which  is  taken  as  the 
positive  side.  The  positive  direction  along  C  is  then  defined  as  the  direction  along  which 


*  If  a  surface  S  has  a  unique  normal  whose  direction  depends  continuously  on  the  points 
of  S,  then  5'  is  called  a  smooth  surface.  If  S  is  not  smooth  but  can  be  subdivided  into 
finitely  many  smooth  portions,  then  it  is  called  a  piecewise  smooth  surface. 


an  observer,  traveling  on  the  positive  side  of  S,  would  proceed  in  keeping  the  enclosed  area 
to  his  left,  (see  Fig. 2  (a)).  vt  is  the  component  of  v  in  the  direction  of  the  tangent  vector 
of  C. 

If  the  direction  cosines  of  the  unit  vector  n  normal  to  the  surface  S  are  a,  j3t  and  7, 
and  if  v  =  vxi  +  v2j  +  v^k,  then  Stokes's  theorem  can  be  written  as 

f  f  \{dv3      du'A  ,   (dv\      duz\        a  ,   fdv'i      dvi\  ja 

J  js  [U"  -  &  ) cosa  +  {  87  "  &  ) cos/3  +  (  &  ~  !>; )  ™->\dA 

—  <i    (vi  dx  +  v2dy  +  v3  dz)  (5) 

If  we  restrict  ourselves  to  the  x-y  plane,  then  Stokes's  theorem  simplifies  to  the  form 

//(^-^f)**-£(",*+*"*)  <6) 

whicli  is  essentially  a  statement  of  Green's  theorem  [8].  For  the  above  equation  the  positive 
direction  of  travel  along  the  closed  curve  C  is  shown  in  Fig. 2  (b).  This  directly  followed 
from  Eq.(5)  where  we  substituted  (a,/?, 7)  =  (ii/2,ir/2,Q).  We  may  change  the  direction  of 
the  closed  curve  C  in  Eq.(6)  by  using  (a, £,7)  =  (n/2, 7r/2. 7r)  m  Eq.(5).  This  will  lead  to  a 
change  in  sign  of  the  surface  integral  in  Eq.(G). 

Another  important  theorem  that  will  serve  as  an  important  tool  for  our  analysis  deals 
with  the  path  independence  of  line  integrals.  This  theorem  is  formally  stated  next  [8] 

Theorem  2:  Let  v  -  vxi  +  v2j  +v3k,  and  let  vu  v2)  and  w3  be  continuous  functions  of 
x,  y,  and  z  in  a  domain  D  of  space.  Then  the  line  integral 


L 


(v\  dx  +  v2dy  +  v-sdz)  (7) 

<c 

is  independent  of  path  if  and  only  if  the  differential  form  under  the  integral  sign  in  exact 
in  D,  or  equivalently  the  integral  is  zero  for  every  simple  closed  path  in  D,  or  equivalently 
V  x  v  -  0,  everywhere  in  D. 

From  the  above  theorem  we  see  that  the  necessary  and  sufficient  condition  for  the 
exactness  of  the  differential  form  under  the  integral  sign  in  Eq.(7)  is 

(s) 


dv2       dv$ 

dv$       dv 1 

dv\ 

dv2 

dz         dy  ' 

dx        dz 

dy 

dx 

2.2  Properties  of  nonholonomic  systems 

In  this  section  we  discuss  some  of  the  important  properties  of  nonholonomic  systems. 
These  properties  will  aid  us  to  develop  the  motion  planning  schemes  in  the  section  4. 

In  section  1  we  mentioned  that  nonholonomic  constraints  are  nonintegrable  expressions 
of  the  form  as  in  Eq.(l)  that  cannot  be  simplified  into  expressions  of  the  form  as  in  Eq.(2). 
To  further  our  discussion,  we  consider  again  the  example  of  the  disk  rolling  without  slipping 
on  a  flat  surface  whose  first  constraint  equation  is 


dx  —  r  sin  udO  =  0 


(9) 


The  above  constraint  is  not  an  exact  differential  since  there  exists  no  function  <£(x,  a,0) 
such  that  Eq.(9)  can  be  reduced  to  the  form 

dx  da  dO 

Furthermore,  Eq.(9)  cannot  be  multiplied  by  an  integrating  factor  to  yield  an  exact  dif- 
ferential. Hence  it  is  not  integrable*.  It  can  be  shown  that  the  necessary  and  sufficient 
condition  for  the  integrability  of  the  differential  equation 


v  j  dx  -f  V2  dy  +  v$  dz  —  0 


is  that 


Vl 


dv2 
~dz~ 


dv2 
dx 


0 


(10) 


dy  J  \  dx        dz  J  \  dy 

Applying  this  criterion  to  Eq.(9)  we  confirm  that  the  expression  is  not  integrable.  In  the 
more  general  case,  the  necessary  and  sufficient  condition  that  the  differential  constraint  in 
n  variables  (Ince,  1956) 


v\  dx\  +  V2  dx2  +  •  ■  ■  +  vn  dxn  =  0 
is  integrable,  is  that  the  set  of  equations 


dv^ 
dx) 


dux 
dx 


+  v     (*± 


dvu 
dxx 


+  vx 


dvu 

dx„ 


dv. 


dx,, 


-  0 
(A,/i,f  =  l,2,"-,n) 


are  satisfied  simultaneously,  and  identically. 


(H) 


The  nonholonomic  property  of  a  dynamical  system  can  also  be  ascertained  from  the 
noninvolutive  property  of  the  distribution  that  spans  the  tangent  space  of  the  system,  using 
Probenius's  theorem.  If  Xx  e  Rn,  i  =  1,2, •••  ,m  denote  the  vector  fields  of  the  system,  then 
the  distribution  A  =  span{Xx,X2, ■■  ■, Xm)  is  involutive  if  and  only  if  A  is  closed  under 
Lie  bracket  operations.  Otherwise,  the  system  is  noninvolutive  or  nonholonomic.  In  the 
case  of  the  rolling  disk, 


r  sin  q 


v    A       0  I      Y    A   |  r  cos  a 
I  0  |  ,A2=  j 


[A"lfX2]  =  (  -Q- -  J  Xi  -  f  -7— -  j  X2  =  (r  cosq      -rsina      0     0)J 


(12) 


*  A  differential  expression  is  integrable  if  it  is  an  exact  differential  or  can  be  converted 
into  an  exact  differential  after  multiplying  with  an  integrating  factor. 

6 


Clearly,  Xu  X2)  and  their  Lie  bracket  [Xi,X2]  are  linearly  independent.  This  reconfirms 
that  the  rolling  disk  is  a  noninvolutive  or  a  nonholonomic  system. 

The  discussion  in  this  section  so  far  enables  us  to  ascertain  the  nonholonomy  of  a 
dynamical  system  from  its  differential  constraints.  We  now  investigate  the  manifestation 
of  these  nonholonomic  constraints. 

If  a  dynamical  system  is  described  by  n  generalized  coordinates,  and  m  holonomic 
constraints  of  the  form  as  in  Eq.(2),  the  motion  of  the  system  is  always  confined  to  a 
manifold  or  surface  of  dimension  (n  -  m),  which  is  equal  to  the  number  of  degrees  of 
freedom  of  the  system.  Then,  if  we  specify  the  (n  -  m)  independent  variables,  it  is  possible 
to  uniquely  determine  the  remaining  m  dependent  variables.  This  is  not  true  when  the 
constraints  are  nonholonomic  or  nonintegrable  expressions  of  the  form  as  in  Eq.(l).  The 
kinematic  effect  of  a  nonholonomic  constraint  is  to  constrain  the  direction  of  the  allowable 
motions  at  any  given  point  in  the  configuration  space.  But  this  does  not  reduce  the  number 
of  dimensions  in  the  configuration  space,  nor  does  it  limit  the  variety  of  configurations 
available  to  the  system.  As  a  direct  consequence,  given  the  values  of  the  independent 
variables,  it  is  not  possible  to  uniquely  specify  the  values  of  the  dependent  variables  of  a 
nonholonomic  system.  When  the  independent  variables  take  one  set  of  values  from  another, 
the  change  in  the  dependent  variables  depend  upon  the  path  taken  by  the  independent 
variables.  Quite  naturally,  if  the  independent  variables  travel  along  a  closed  path,  the 
values  of  the  dependent  variables  at  the  beginning  and  end  of  the  path  are  usually  not  the 
same. 

The  above  mentioned  property  of  a  nonholonomic  system  is  better  understood  by  the 
use  of  Theorem  2  on  line  integrals.  Comparing  Eq.(8)  (conditions  for  exactness)  to  Eq.(lO) 
(conditions  for  integrability),  or  directly  from  the  definition  of  integrability,  we  know  that 
exactness  implies  integrability*.  Therefore  it  follows  that  a  nonintegrable  expression  is  not 
exact.  Consider  now  a  nonholonomic  system  where  one  of  the  dependent  variables  is  p  and 
it  is  constrained  by  the  differential  expression  dp  =  v^  dx  +  v2  dy  -\-v-sdz  ,  where  x,  y,  and  z 
are  the  independent  variables.  vu  v2)  and  v-s  are  continuous  functions  of  x,  y,  and  z.  Since 
the  system  is  nonholonomic  or  nonintegrable,  the  differential  form  vx  dx  +  v2  dy  +  v3dz  is  not 
exact.  Therefore  it  follows  from  Theorem  2  that  the  change  of  p  is  path  dependent,  and 
this  change  is  not  zero  for  every  closed  path.  This  suggests  the  following. 

1 .  It  is  possible  to  change  the  coordinates  of  the  dependent  variable  p  of  the  nonholonomic 
system  using  appropriate  closed  trajectories  of  the  independent  variables,  and 

2.  There  may  exist  some  closed  paths  for  which  the  path  dependent  integral  in  Eq.(7) 
will  be  zero  for  the  nonholonomic  system. 

On  the  basis  of  statement  1  discussed  above,  we  now  assume  that  there  exists  some  closed 


*  Integrability  however  does  not  imply  exactness  because  an  integrable  differential  ex- 
pression could  have  become  exact  only  after  it  was  multiplied  by  some  multiplicating 
factor. 


trajectory  C  of  the  independent  variables  x,  y,  and  z  that  produce  a  change  in  the  dependent 
variable  p  by  some  desired  amount  Ap.  If  (x0,y0,zo)  be  any  point  on  this  closed  trajectory 
and  if  the  initial  configuration  of  the  system  is  (x0>2A),zo,po),  tnen  ^eT  tne  system  moves 
along  C  once,  its  configuration  will  be  (x0,yo,2o,po  +  Ap)  (refer  to  Fig. 3  (a)).  If  the  closed 
curve  C  was  traversed  in  the  opposite  direction,  then  the  final  configuration  of  the  system 
would  have  been  (x0,  yQ,  z0,  p0  -  Ap).  Now  consider  the  initial  configuration  of  the  system  to 
be  (x',  y',  z',po),  such  that  (x',y't  z')  does  not  lie  on  C.  Let  P  be  any  path  segment  connecting 
the  point  (x',y',z')  and  any  point  (x0)yo,  20)  on  the  closed  curve  C.  Let  6p  denote  the  change 
in  the  dependent  variable  p,  as  x,  y,  and  z  move  along  the  path  segment  P  from  (x',y\z') 
to  (xo,2A),2o)-  Then,  if  the  system  moves  from  the  initial  configuration  {x' ,y' ,z' ,p0)  to  the 
closed  curve  C  along  P,  then  moves  once  along  the  closed  curve  C,  and  finally  retraces  the 
path  P  backwards,  the  configuration  of  the  system  at  the  end  of  the  path  (see  Fig.3  (b))  will 
be  (x'.j/,  z\po  +  Ap).  This  is  true  because  the  surface  integral  of  the  closed  curve  beginning 
and  ending  at  the  point  (x',y',z')  is  equal  to  the  surface  integral  of  the  closed  curve  C. 
From  this  discussion  it  follows  that  the  closed  curve  C  that  can  bring  about  the  desired 
change  in  the  dependent  variable  can  lie  anywhere  in  the  space  defined  by  the  independent 
generalized  coordinates  -  it  does  not  have  to  pass  through  the  initial  configuration  of  the 
system.  Of  course,  it  would  be  simpler  to  plan  a  closed  path  passing  through  the  initial 
configuration  of  the  system  but  then  such  a  path  may  not  be  feasible  due  to  singularity 
problems.  We  will  discuss  the  singularity  problem  in  the  particular  situation  of  a  rolling 
disk,  in  section  4. 

In  regards  to  statement  2  discussed  above,  we  would  just  like  to  mention  that  closed 
trajectories  of  the  independent  variables  that  result  in  closed  trajectories  of  the  dependent 
variables  (repeatable  motion)  will  be  of  importance  to  us  in  the  context  of  nonholonomic 
motion  planning.  Repeatability  in  the  motion  may  be  simply  a  desirable  property  as  in 
the  case  of  space  robots,  or  may  even  be  used  for  singularity  avoidance  as  in  the  case  of  a 
rolling  disk.  In  section  4  we  will  investigate  into  the  repeatability  of  the  motion  of  a  space 
robot  and  of  a  rolling  disk. 

3.  Issues  related  to  Nonholonomic  Motion  Planning 

The  configuration  space  of  a  nonholonomic  system  is  described  by  the  set  of  its  in- 
dependent and  the  dependent  variables.  The  task  of  nonholonomic  motion  planning  is 
to  generate  trajectories  of  the  independent  variables  that  will  take  the  system  from  its 
current  configuration  to  some  desired  configuration.  In  the  context  of  the  rolling  disk,  the 
motion  planning  would  therefore  refer  to  the  generation  of  the  0  and  a  trajectories  that 
will  take  the  system  from  some  initial  configuration  (xityi,Oitai)  to  some  final  configuration 

The  question  that  naturally  arises  in  the  context  of  motion  planning  is  related  to  the 
reachability*  of  the  system.  In  the  case  of  the  rolling  disk,  we  know  that  any  configuration 
of  the  system  is  reachable  from  any  other.  This  follows  from  our  discussion  in  section  1.  For 


A  number  of  researchers  like  [10],  prefer  to  use  the  term  controllability  to  reachability, 


a  multibody  car  system  (a  car  with  n  trailers),  a  mathematical  proof  of  the  controllability 
(reachability)  was  provided  in  [1].  For  nonholonomic  systems  in  general,  the  reachability 
can  be  ascertained  by  constructing  the  control  Lie  algebra  and  then  using  the  controlla- 
bility theorem  for  nonlinear  systems  [3j.  The  control  Lie  algebra  is  the  smallest  involutive 
distribution  containing  the  span  of  the  vector  fields  of  the  system  and  closed  under  Lie 
bracket  operations.  If  the  rank  of  this  Lie  algebra  is  full  at  some  configuration  C  of  the 
system,  then  there  exists  a  neighborhood  N  of  C  whose  points  represent  configurations 
reachable  by  the  system  from  C  along  admissible  paths.  Clearly,  this  condition  is  a  local 
condition.  If  this  condition  holds  good  at  every  point  in  the  configuration  space,  then  any 
configuration  of  the  system  is  reachable  from  any  other  using  admissible  paths. 

The  controllability  (reachability)  of  a  number  of  simple  nonholonomic  systems  has 
been  verified  using  the  approach  discussed  above.  However,  for  complicated  systems  like 
a  6DOF  free-flying  space  robot  [12],  this  approach  is  not  useful.  In  the  next  section  we 
will  generate  admissible  trajectories  for  simple  nonholonomic  systems  using  an  algorithm 
based  on  Stokes's  Theorem.  For  the  two  nonholonomic  systems  that  we  have  considered 
in  section  4,  we  find  that  reachability  can  be  easily  concluded  directly  from  our  algorithm. 

An  important  feature  of  motion  planning  algorithms  should  be  their  ability  to  plan 
admissible  trajectories  amidst  additional  constraints.  In  the  case  of  robot  manipulators 
these  additional  constraints  may  appear  in  the  form  of  obstacles  in  the  workspace  or  limits 
imposed  on  the  angular  displacement  of  the  joints.  In  the  case  of  the  rolling  disk  admissible 
trajectories  may  have  to  be  planned  by  avoiding  obstacles  in  the  x-y  plane.  Collision-free 
trajectories  or  trajectories  amidst  additional  constraints  have  been  planned  using  artificial 
potential  functions  [7],  [13].  In  comparison  to  these  approaches,  the  algorithm  discussed  in 
this  paper  has  a  global  attribute.  This  feature  will  be  evident  in  the  next  section  through 
examples. 

While  discussing  the  properties  of  nonholonomic  systems  in  section  2,  we  realized 
that  closed  trajectories  of  the  independent  variables  more  often  result  in  a  change  in  the 
dependent  variables.  This  will  provide  a  basis  for  our  motion  planning  algorithm,  where 
closed  trajectories  of  the  independent  variables  will  be  suitably  planned  in  order  to  produce 
a  desired  change  in  the  dependent  variables.  In  particular  situations  we  may  however  be 
interested  in  finding  closed  trajectories  of  the  independent  variables  that  also  produce 
closed  trajectories  of  the  dependent  variables  (repeatable  motion).  Consider  the  example 
of  a  planar  robot  in  space  with  two  links  mounted  in  a  space  vehicle,  as  shown  in  Fig. 4.  In 
this  case,  the  orientation  of  the  space  vehicle  0o  is  the  dependent  variable  while  the  joint 
variables  6X  and  92  are  the  independent  variables.  If  this  robot  is  expected  to  perform  a 
repeatitive  task  in  space,  we  would  expect  all  the  variables  0o,  0\,  and  02  to  move  along 
closed  trajectories.  In  the  next  section,  we  illustrate  repeatability  in  the  case  of  a  two-link 
space  robot,  and  for  a  rolling  disk.  In  the  case  of  the  rolling  disk  repeatable  motion  leads 
to  the  generation  of  singularity  free  trajectories. 


and  adhere  to  the  terminology  used  in  [14]. 


4.  Nonholonomic  motion  planning  using  Stokes's  theorem 

4.1  Example  1:  A  planar  space  robot 

In  this  section  we  illustrate  our  algorithm  for  nonholonomic  motion  planning  using 
Stokes's  theorem,  through  the  example  of  a  2-DOF  planar  space  manipulator.  We  consider 
a  planar  free-flying  space  manipulator  consisting  of  two  links  mounted  on  a  space  vehicle, 
as  shown  in  Fig.4.  Such  a  system  can  be  described  by  five  coordinates:  io,  yo,  and  0O 
representing  the  position  of  the  center  of  mass  and  the  orientation  of  the  space  vehicle, 
and  6i  and  02  representing  the  joint  angles  of  the  manipulator.  The  variables  x0  and  y0  can 
be  eliminated  by  using  the  holonomic  constraints  due  to  linear  momentum  conservation. 
The  2-DOF  system  is  then  described  by  three  generalized  coordinates  0,,  i  =  0,1,2,  and 
one  nonholonomic  constraint  due  to  the  conservation  of  angular  momentum  given  by  the 
relation 

do  =  ^(a0x+bd2)  (13) 

where, 


A  =  (rm,  +  m2jil\  +  -m\ll  -  (m0  +  -m1)m2/i/2  cos02  -  M  (  I  +  (-mj  +  m2)l\  +  -m2l\ 
a  =  -A  -  M  I0 

A    ■-    /•  ]         ,2    ,    1         ,   ,    _.n  \         1      2,2        1         ,1 


fc=M   ( /2  + -m2/2  +  ^rn2/!/2cos02  j  - -m2/2  - -m2(-mi -h  m2)/!/2cos02  (14) 

and  where,  m0)  mi  and  m2  are  the  masses  of  the  space  vehicle  and  the  two  links,  /0,  I\  and 
h  are  the  moments  of  inertia  of  the  space  vehicle  and  the  two  links  about  their  center  of 
masses,  l\  and  i2  are  the  length  of  the  two  links,  and  M  =  (m0-i-m1-f-m2)  and  /  =  (/o+^i  +  /2)- 

The  configuration  of  the  space  manipulator  can  be  described  by  (0o,0i,02)-  Then 
the  path  planning  problem  is  to  find  suitable  trajectories  for  0X  and  02  that  will  change 
the  current  configuration  of  the  space  manipulator  to  some  desired  configuration.  This 
immediately  raises  the  question  pertaining  to  the  reachability  of  the  system  -  is  it  possible 
to  plan  trajectories  that  will  take  the  space  manipulator  from  any  initial  configuration  to 
any  final  configuration.  The  answer  to  this  question  is  yes,  and  can  be  obtained  directly 
from  our  path  planning  algorithm,  discussed  below. 

Let  the  arbitrary  initial  and  desired  configurations  of  the  space  manipulator  be  denoted 
hy  (0oi,0i,,02«)  and  (0O/,  0lf,  02j)  respectively.  We  first  converge  the  joint  variables  0U  and  02, 
(the  independent  variables)  to  their  desired  values  0i/  and  02/  respectively.  In  this  process, 
let  the  orientation  of  the  space  vehicle  (the  dependent  variable)  drift  from  0O,  to  some  value 
80d.  The  task  is  now  to  plan  a  cyclic  motion  for  the  joints  of  the  manipulator  such  that  the 
orientation  of  the  space  vehicle  changes  from  0Od  to  0O/  while  the  joint  angles  come  back  to 
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their  desired  configuration.  The  associated  reachability  problem  can  be  solved  if  we  can 
show  that  it  is  possible  to  change  the  orientation  0O  by  an  arbitrary  amount  using  cyclic 
motion  of  the  joints,  at  any  joint  configuration  0X  and  02.  For  cyclic  motion  of  the  joints, 
the  change  in  0O  is  represented  as 

f  d00=  I  ^(adOi+bdO,)  (15) 

where,  A,  a,  and  b  have  been  defined  by  Eq.(14),  and  C  in  Eq.(15)  is  a  closed  curve  in  the 
0r02  plane  that  we  will  suitably  choose.  Using  Green's  theorem  given  by  Eq.(6),  the  above 
equation  is  simplified  to 


A']  v2;2    ,     *       2i2         h4    (1    ,    /I.      , \,2    ,     *  ,2 


A  =  (rmj  +  m2yi\  +  -m$l$  -  M   (  /  +  (-m,  +  m2)/f  +  jm^ 

£?  =  -(m0  +  -mi)m2/i/2  (I7) 

where,  the  expressions  for  a  and  b  were  substituted  from  Eq.(14),  and  5  is  the  surface  in 
the  0j-02  plane  confined  within  the  closed  curve  C.  Let  the  desired  change  in  0O  be  denoted 
as  0O.  Then  the  path  planning  problem  reduces  to  the  proper  selection  of  the  area  S  in  the 
0r02  plane  such  that  the  following  equality  is  satisfied 

If  we  choose  a  rectangular  path  in  the  0\-02  plane,  such  that  the  sides  of  this  rectangle 
are  parallel  to  the  0i  and  02  axes,  then  the  above  identity  reduces  to 


(0iu-0n) 


1  1 


=  *  (19) 


A+  B  cos  02u       A  +  D  cos  02* 
where,  0k  and  0iu  denote  the  lower  and  upper  extremities  of  0i  in  the  rectangular  path 
while  02i  and  02u  denote  the  same  for  02. 

The  reachability  of  the  system  can  be  proven  by  showing  that  there  exists  a  surface 
5  such  that  the  equality  in  Eq.(18)  can  be  satisfied  for  any  arbitrary  value  of  k.  The 
initial  values  of  0t  and  02  does  not  necessarily  have  to  lie  on  the  boundary  of  this  surface 
S.  This  follows  from  our  discussion  in  section  2.2.  We  first  note  that  if  the  identity  in 
Eq.(18)  can  be  satisfied  for  some  value  of  k  by  traveling  along  the  boundary  of  the  area  S 
in  the  positive  direction,  then  the  same  identity  can  be  satisfied  for  -A;  by  simply  traveling 
along  the  boundary  in  the  negative  direction.  Furthermore,  if  the  identity  can  be  satisfied 
for  some  value  of  k  by  traveling  once  along  the  boundary  of  S,  then  the  identity  can  be 
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satisfied  for  the  value  nk,  n  —  1,2,  •  •  •,  by  traveling  n  times  along  the  boundary  in  the  same 
direction.  Clearly,  the  reachability  problem  reduces  to  showing  that  the  identity  in  Eq.(18) 
can  be  satisfied  for  any  value  of  k  e  (0,e),  where  e  is  some  positive  small  number.  For  a 
rectangular  area  5,  this  is  easy  to  prove.  We  can  always  choose  the  values  of  62i  and  02u  in 
Eq.(19)  such  that  the  quantity  a  defined  as 


Q  — 


( ! ! ) 

\  A  +  B  cos  02u       A  +  D  cos  0U  ) 


is  not  equal  to  zero.  Then  it  is  quite  obvious  that  (0lu  -  0U)  can  be  chosen  to  be  equal  to 
k/a  such  that  the  identity  in  Eq.(19)  can  be  satisfied  for  any  value  of  k  e  [0,  e). 

We  now  illustrate  our  path  planning  algorithm  with  the  help  of  a  simple  example.  We 
consider  a  space  manipulator  (refer  to  Fig. 4)  of  material  alluminum,  whose  kinematic  and 
dynamic  parameters  are  given  below  in  SI  units: 

Kinematic  and  Dynamic  parameters 

Mass  Inertia  Length 

Vehicle  27.440  1.520 

Link-1  5.380  0.115  lx  =  0.50 

Link-2  2.640  0.028  lx  =  0.35 

Let  the  initial  configuration  of  the  system  be  {0oXi0Xx,62x)  =  (0.0, 15.0, 15.0)  degrees,  and  the 
final  configuration  be  {0Of,0lf,02J)  =  (-20.0,45.0,0.0)  degrees.  We  first  converge  6X  and  02 
from  their  current  values  to  their  desired  values  using  the  straight  line  trajectory  OA,  as 
shown  in  Figs. 5  and  6.  In  this  process,  the  orientation  of  the  space  vehicle  drifts  from 
0.0  degrees  to  0o<i  =  -12.87  degrees,  as  shown  in  Fig. 6.  Therefore,  00  =  (0o/  -  0Od)  =  -7.13 
degrees,  or  -0.1244  radians.  We  now  plan  a  cyclic  motion  of  the  joints  such  that  after 
three  complete  cycles  of  the  joint  motion  the  orientation  of  the  space  vehicle  changes 
by  the  desired  amount.  Then,  for  each  cycle,  the  required  change  of  orientation  would  be 
-0.1244/3.0  =  -0.0415  radians.  Using  this  value  for  0O,  the  value  of  k  in  Eq.(18)  is  computed 
to  be  -7.696  x  10~4.  With  the  complete  liberty  to  choose  the  closed  path  in  the  joint  space, 
we  choose  the  simple  directed  path  given  by  the  rectangle  ABCD  in  Fig.5.  The  closed 
curve  is  chosen  such  that  the  intermediate  configuration  of  the  system  -  point  A,  lies  on 
this  closed  curve.  The  surface  integral  in  Eq.(18)  then  simply  reduces  to 


J   JABCDd\A+Bcos02)  d°x      (<?1/     °u)  \A  +  Bcos02J      A  +  Bcos02l) 


=  (x-^{aTW^ 


i 

cos  7       A  +  B  cos  0 


where,  A  and  7  have  to  be  chosen  appropriately.  We  choose  A  =  125.0  degrees  i.e.  2.181 
radians.  Then  using  the  values  of  A  =  -89.848  and  B  =  -13.92  from  Eq.(17)  and  the  table 
above,  we  obtain  the  value  of  7  =  0.596  radians  or  53.36  degrees.  For  this  choice  of  A  and  7, 
the  evolution  of  all  the  configuration  variables  for  the  path  OABCD  are  shown  in  Fig.6. 
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We  would  like  to  mention  a  few  points  at  this  juncture.  For  the  same  set  of  values  of 
7  and  A,  there  are  infinite  paths  that  will  produce  the  same  change  in  the  orientation  0o. 
As  for  example,  the  directed  paths  OABCDA  (taken  from  Fig. 5)  and  OADMNA  in  Fig.7 
will  produce  the  same  change  in  the  orientation  0o.  In  both  these  cases  the  intermediate 
configuration  (0od,0i/,02/)  -  point  A,  lie  on  the  closed  curves  ADCD  and  ADMN.  This  only 
simplifies  the  motion  planning  problem  but  is  not  a  necessary  condition,  as  we  have  already 
discussed  in  section  2.2.  Area  ADMN  can  be  concieved  as  a  translation  of  the  area  ADCD 
to  the  left  along  BA.  In  fact,  the  translation  of  the  area  ABCD  by  any  amount  along  the 
line  BA  will  result  in  feasible  closed  trajectories.  This  follows  directly  from  the  expression 
for  the  surface  integral  in  Eq.(20).  We  see  that  the  surface  integral  in  Eq.(20)  depends 
upon  the  difference  of  the  values  of  QXj  and  0XXi  but  not  individually  on  0X/  and  0Xl.  There 
are  also  infinite  other  combinations  of  A  and  7  values  that  we  can  choose  to  satisfy  Eq.(20). 
And  there  is  even  more  flexibility  when  we  can  suitably  choose  the  number  of  cycles  of 
joint  motion  that  will  produce  the  desired  change  in  the  orientation  0O.  As  for  example, 
in  the  particular  situation  discussed  above,  the  directed  path  AXYZ  can  bring  about  the 
same  change  in  0O  in  four  cycles  of  the  joint  motion  that  the  paths  ABCD  and  ADMN  can 
bring  about  in  three  cycles.  The  path  AXYZ  was  obtained  by  choosing  7  =  75.0  degrees 
which  led  to  the  value  of  A  =  76.084  degrees. 

We  next  consider  the  practical  situation  where  the  trajectory  of  the  system  may  have 
to  be  planned  subject  to  the  joint  limits  \0X\  <  120  degrees.  Then  for  the  above  example,  the 
trajectories  ADMN  and  AXYZ  in  Fig.7  would  be  feasible  whereas  the  trajectory  ABCD 
would  not  be  feasible.  It  is  quite  clear  that  our  path  planning  algorithm  provides  us  with 
the  flexibility  in  choosing  trajectories  that  can  satisfy  additional  constraints,  like  joint 
limits  in  the  case  of  space  robots. 

We  complete  this  section  with  a  discussion  on  repeatable  motions  of  the  space  robot. 
In  particular  situations,  a  space  robot  may  be  expected  to  perform  a  repeatative  task 
in  space.  In  such  a  situation  the  end-effector  of  the  robot  as  well  as  the  configuration 
variables  of  the  robot  will  all  have  to  move  along  closed  trajectories.  If  the  joints  of  the 
robot,  shown  in  Fig. 4,  move  along  closed  trajectories,  the  dependent  variables  x0  and  y0 
will  always  move  along  closed  trajectories  because  of  the  holonomic  nature  of  the  linear 
momentum  constraints.  The  dependent  variable  0O  will  however  not  move  along  a  closed 
trajectory  in  the  general  case.  If  the  net  change  in  0O  were  also  to  be  zero  as  the  joints 
moved  along  a  closed  rectangular  path,  then  from  Eq.(19),  the  necessary  conditions  that 
would  have  to  be  satisfied  are  &Xu  -  6U,  or  cos02u  =  cos02,  (assuming  a  rectangular  path  in 
the  0x-02  plane).  The  first  condition  leads  us  to  the  trivial  case  where  the  first  joint  of  the 
robot  will  have  to  be  kept  fixed.  The  second  condition  tells  us  that  repeatability  is  assured 
for  02li  +  02i  =  2n7r,  n  =  0,  ±1,  ±2,  •  •  •.  In  the  previous  example  that  we  considered,  we  found 
that  we  had  a  significant  amount  of  flexibility  in  choosing  the  closed  trajectories.  Though 
the  condition  02u  -f  02l  =  2nn,  n  =  0,±l,±2,  •  •  •,  will  restrict  our  choices,  we  should  still  be 
able  to  choose  from  a  variety  of  paths  that  will  produce  repeatable  motion. 
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4.2  Example  2:  A  disk  rolling  without  slipping  on  a  flat  surface 

We  revisit  the  classical  example  of  the  disk  rolling  without  slipping  on  a  fiat  surface 
and  this  time  we  apply  our  algorithm  for  its  nonholonomic  path  planning.  The  configu- 
ration space  of  the  rolling  disk,  as  discussed  in  section  1,  is  described  by  (x,y,9,a).  The 
independent  variables  of  the  system  are  9  and  a,  and  x  and  y  are  the  dependent  variables 
constrained  by  the  expressions  given  in  Eq.(3).  While  planning  a  path  from  an  initial 
configuration  of  the  disk  (xl,yl,Ol,al)  to  some  desired  configuration  (xf,y/,0j,af),  we  first 
converge  the  independent  variables  to  their  desired  values  using  some  simple  trajectory 
without  being  much  concerned  about  the  evolution  of  the  dependent  variables.  This  tra- 
jectory may  however  be  important  so  as  to  avoid  a  singularity  problem,  which  we  shall 
discuss  soon.  At  the  end  of  this  trajectory  (path  segment  OP  in  Fig. 8),  the  dependent 
variables  are  assumed  to  drift  to  values  xd  and  yd.  Now  the  task  is  to  plan  a  closed  path 
for  the  independent  variables  0  and  a  such  that  the  dependent  variables  x  and  y  change 
their  values  from  xd  and  yd  to  xj  and  y/  respectively.  Let  C  be  such  a  closed  path  in  the 
0-a  plane.  Then  the  change  of  the  variables  x  and  y  are  represented  as 

(if  —  xd)  —  <f>  r  sin  adO  =    /    /   — r  cos  a  dO  da  (21 ) 

(Vf  ~  Vd)  —  <P  T  cos  add  —  r  sin  a  dO  da  (22) 

where,  we  applied  Green's  theorem,  given  by  Eq.(6),  to  convert  the  line  integral  into  the 
surface  integral.  Therefore,  S  is  the  surface  in  the  0-a  plane  within  the  closed  curve  C.  We 
choose  the  closed  curve  C  as  the  directed  rectangular  path  PQRSP  as  shown  in  Fig. 8.  For 
this  rectangular  path  PQRSP}  Eqs.(21)  and  (22)  yield 


{xs  -xd)  =  -2arsin(b/2)  cos(a,  +  fc/2)  (23) 

(yf  -yd)=2ar sin(ty2)  sin(a/  +  6/2)  (24) 

Assuming  b  /  2n7r,  n  —  ±1,  ±2,  •  •  •,  we  solve  for  a  and  b  as  follows 

b  =  2[-a/  +  arctan 2 (j//  -yd,xd  -  x/)] ,  0  <  b  <  4n  (25) 

B=  «*>-*?  + % -»rr,  a>0  (26) 

2rsin(fc/2) 

As  the  disk  would  move  along  the  sides  QR  and  SP  of  the  rectangular  path  PQRSP  in 

Fig. 8,  the  value  of  a  would  have  to  change  in  the  absence  of  rolling.  This  may  not  be  simple 

to  achieve  in  practice,  as  for  example  in  the  case  of  an  unicyclist.    We  therefore  modify 

our  rectangular  path  to  the  path  PQMNP  in  Fig. 8  where  a  would  change  only  when  the 

disk  is  rolling.  It  is  easy  to  show  that  surface  the  integral  in  Eq.(21),  as  in  Eq.(22),  will 

have  the  same  value  when  the  closed  curve  C  is  the  rectangle  PQRSP  or  the  parallelopiped 

PQMNP. 
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Equation  (26)  has  a  singularity  for 

aict&n2  (yj  -yd,  Xd  -  x/)  —  a/  (27) 

and  there  are  three  simple  ways  to  overcome  this  singularity  problem.  One  way  would  be 
to  arrive  from  the  initial  configuration  to  a  different  intermediate  configuration  i.e.  at  a 
different  set  of  values  xd  and  yd,  that  would  not  satisfy  Eq.(27).  This  would  require  us 
to  choose  a  different  trajectory  from  O  to  P  (see  Fig. 8).  The  second  alternative  would  be 
to  set  an  intermediate  goal  x'f)  y'f  and  first  move  to  this  configuration  from  xd,  yd  using 
cyclic  motion  of  the  independent  variables.  Then  the  remaining  task  would  be  to  plan  a 
second  cyclic  motion  of  the  independent  variables  such  that  the  dependent  variables  would 
converge  to  xf,  yf  from  their  values  x'f,  y'..  The  smart  alternative  would  be  to  follow  the 
three  step  procedure  explained  below  and  diagrammatically  shown  in  Fig. 9. 

1  Change  the  present  configuration  variables  0f  and  a}  to  some  other  values  0'f  and  a'f 
using  any  trajectory  OR,  such  that  a'f  ^  af,  at  R.  Ignore  the  evolution  of  the  variables 
x  and  y  that  take  the  new  values  x'd,  y'd  at  R  from  their  values  xd,yd  at  O. 

2  Construct  any  closed  path  C  passing  through  R,  that  will  change  the  x  and  y  variables 
by  amounts  (xf  -xd)  and  (yj  -  yd)  respectively.  If  we  choose  a  rectangular  path,  or  the 
equivalent  parallelogram  path,  then  the  dimensions  of  this  path  would  be  computed 
from  Eqs.(24)  and  (25)  only  by  replacing  af  in  Eq.(24)  by  a'f.  Move  along  this  closed 
trajectory  once  to  come  back  to  R  where  0  =  0'f  and  q  =  a'f. 

3  Retrace  backwards  the  trajectory  OR  to  move  from  R  to  O.  At  O  the  configuration  of 
the  system  would  be  (xf,yf,0f,af). 

The  above  procedure  for  singularity  avoidance  follows  from  our  discussion  in  section 
2.2.  This  procedure  is  also  recommended  for  avoiding  points  close  to  singular  points. 
At  points  close  to  the  singularity,  trajectories  tend  to  become  infeasible  due  to  the  large 
magnitude  of  a,  as  evident  from  Eq.(26). 

We  are  now  convinced  that  any  configuration  of  the  rolling  disk  (xj,yj,0Jtaj)  can  be 
reachable  from  any  initial  configuration  (xltyltOlt  a,)  by  using  the  motion  planning  algorithm 
discussed  in  this  section.  Though  in  certain  situations,  there  may  be  a  singularity  problem, 
this  problem  can  be  easily  overcome.  The  singularity  that  we  may  encounter  is  however 
not  a  physical  singularity,  it  is  rather  an  algorithmic  singularity.  The  singularity  problem 
can  be  completely  removed  by  adopting  a  slightly  different  algorithm.  This  algorithm,  to 
be  discussed  shortly,  uses  the  repeatable  motion  of  one  of  the  dependent  variables.  This 
algorithm  will  further  strengthen  our  conviction  on  the  reachability  issue  of  the  rolling 
disk. 

We  illustrate  our  path  planning  algorithm  with  the  help  of  a  simple  example.  In  this 
example  we  come  close  to  the  singularity  and  we  tactfully  avoid  it  using  the  algorithm 
discussed  above.  Consider  a  disk  of  radius  r  =  0.25m  which  is  at  its  current  configuration 
(z,,2/,,0l,at)  =  (0.0,0.0,0.0,0.0)  metres,  degrees.  Suppose  the  desired  configuration  of  this 
disk  is  (xf,yf,0/,af)  =  (-0.4,1.0,180.0,22.5)  metres,  degrees.    We  first  converge  the  vari- 
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ables  0  and  a  to  their  desired  values  using  the  simple  straight  line  path  OZ,  as  shown  in 
Fig.  10.  The  coordinates  of  x  and  y  at  the  end  of  this  path  will  be  0.1522  and  0.7654  metres 
respectively  (obtained  through  numerical  simulation).  Using  Eqs.(25)  and  (26),  we  solve 
for  a  and  b  as  b  —  0.01808  and  a  —  132.72  radians  respectively.  Clearly,  we  are  close  to  a 
singular  configuration.  In  the  light  of  the  discussion  on  singularity  avoidance,  we  adopt 
the  following  measure.  We  have  the  complete  liberty  to  choose  0'}  and  a'f  (point  R  in 
Fig.9),  with  the  only  restriction  that  a'f  ^  af.  We  choose  (6'f,a'f)  =  (0,,a.)  =  (0.0,0.0).  And 
we  change  6}  and  af  to  0,  and  a,  by  retracing  the  path  OZ  backwards  (see  Fig.  10).  We 
therefore  come  back  to  the  initial  configuration  where  (x,y,9,a)  =  (xx,yl,9x,al).  Now  we 
substitute  the  value  of  a,  in  place  of  af  in  Eq.(25)  and  solve  for  a  and  6  from  Eqs.(25) 
and  (26).  We  obtain  6  =  0.8034  radians  or  46.03  degrees  and  a  =  3.068  radians  or  175.78 
degrees.  In  Fig.  10,  the  path  OPQRO  is  the  closed  path  constructed  with  these  values  of  a 
and  b.  Due  to  the  motion  along  this  closed  path  the  change  in  the  x  and  y  coordinates  will 
be  (xf  -  xd)  =  (-0.4  -  0.1522)  =  -0.5522  metre  and  (yf  -  yd)  =  (1.0  -  0.7654)  =  0.2346  metre 
respectively.  Therefore  the  coordinates  at  O  of  the  dependent  variables  after  the  motion 
along  the  closed  path  will  be  x  =  -0.5522  and  y  =  0.2346  metres.  We  finally  trace  the 
straight  line  path  from  O  to  Z.  Due  to  this  motion  the  change  in  the  x  and  y  coordinates 
will  be  (xd  -  x.)  =  (0.1522  -  0.0)  =  0.1522  metre  and  (yd  -  yx)  =  (0.7654  -  0.0)  =  0.7654  metre 
respectively.  Therefore,  the  coordinates  at  Z  will  be  (-0.5522  +  0.1522)  =  -0.4  metre  and 
(0.2346  +  0.7654)  =  1.0  metre  respectively.  The  coordinates  of  the  independent  variables  at  Z 
are  obviously  9  =  180.0  degrees  and  a  =  22.5  degrees.  Looking  back  at  the  entire  motion,  we 
realize  that  the  initial  path  from  O  to  Z  and  back  to  O  is  redundant.  Therefore,  the  path 
that  will  be  sufficient  for  converging  all  the  variables  will  be  OPQROZ,  as  shown  in  Fig.  10. 
The  closed  path  OPQRO  is  chosen  to  be  a  parallelogram  instead  of  a  rectangle  for  reasons 
we  have  already  discussed  earlier  in  this  section.  The  complete  path  OPQROZ  is  quite 
different  from  paths  that  are  generated  in  the  absence  of  singularity.  In  the  absence  of 
singularity,  the  complete  path  consists  of  an  initial  path  segment  followed  by  a  closed  loop 
in  the  0-a  plane.  In  the  particular  example  that  we  have  considered,  we  had  a  singularity 
and  the  complete  path  consisted  of  a  closed  loop  path  followed  by  a  simple  path  segment. 
The  simulation  results  of  this  particular  example  have  been  shown  in  Fig.  11.  The  points 
O,  P,  Q,  R}  and  Z  in  Fig.  11  correspond  to  the  same  points  in  Fig.  10. 

The  singularity  problem  discussed  above  arises  due  to  the  particular  nature  of  our 
algorithm  where  we  converge  both  the  dependent  variables  x  and  y  simultaneously  using 
closed  trajectories  of  the  independent  variables.  We  have  seen  that  this  is  not  at  all 
a  serious  problem.  However,  this  problem  can  be  completely  eliminated  by  adopting  a 
slightly  different  algorithm.  The  idea  behind  this  algorithm  is  to  use  repeatable  motion  of 
one  of  the  dependent  variables.  This  algorithm  is  discussed  next. 

In  our  singularity  free  algorithm,  we  will  first  converge  the  independent  variables  9 
and  a  from  their  initial  values  (0,,a,)  to  their  desired  values  (9f,af).  Let  us  suppose  that 
the  dependent  variables  x  and  y  change  their  coordinates  from  x,  and  yx  to  xd  and  yd 
respectively.  We  will  next  converge  x  to  its  desired  value  x/  using  closed  trajectories  of  the 
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independent  variables  without  being  concerned  about  the  evolution  of  the  y  coordinate. 
Specifically,  we  will  use  Eq.(23),  where  we  will  have  the  liberty  to  choose  a  set  of  values  a 
and  b.  Let  us  suppose  that  the  dependent  variable  y  drift  from  its  previous  coordinate  yd 
to  y'd  in  this  process.  We  will  finally  converge  y  from  its  present  coordinate  y'd  to  its  desired 
value  yf  using  closed  trajectories  of  the  independent  variables  that  will  also  produce  a 
closed  trajectory  of  the  dependent  variable  x,  i.e.  a  repeatable  motion  in  the  x  coordinate. 
From  Eqs.(23)  and  (24)  it  follows  that  the  correct  choice  of  the  variables  a  and  b  for  this 
repeatable  motion  should  be 


Ay 

b=-2a,±nn,  a  =  - - ,    Q/^±(2n+l)7r/2,  n  =  0,l,2,---         (28) 

2  r  cos  a  j 

where  Ay  =  (,y/-y'd),  and  b  will  be  chosen  such  that  6  >  0.  The  magnitude  of  a  can  however 
be  positive  or  negative.  We  will  use  the  absolute  value  of  a  to  construct  the  parallelogram 
path  in  the  0-a  plane.  If  the  sign  of  a  is  obtained  negative  from  Eq.(28),  then  the  use  of 
the  positive  value  will  bring  about  a  change  in  the  y  coordinate  by  an  amount  -Ay  instead 
of  Ay.  This  problem  can  be  simply  solved  by  changing  the  direction  of  travel  along  the 
closed  path.  This  idea  has  been  appropriately  demonstrated  in  the  next  simulation. 

As  an  alternative,  we  could  also  converge  the  dependent  variable  y  before  we  converge 
the  dependent  variable  x.  In  that  case,  after  the  initial  motion  from  the  configuration 
(zi,2/i,0i,a,)  to  the  configuration  {xd,yd,0/,a/),  we  would  suitably  choose  a  and  b  in  Eq.(24) 
such  that  y  converges  from  yd  to  yj.  At  this  stage  we  will  not  be  concerned  about  the 
evolution  of  the  x  coordinate,  which  will  probably  drift  from  xd  to  x'd.  Finally,  we  will 
change  the  variable  x  by  an  amount  Ax  =  (xf  -  x'd)  using  closed  trajectories  of  0  and  a 
that  will  also  produce  a  closed  trajectory  of  y,  i.e.  a  repeatable  motion  in  the  y  coordinate. 
From  Eqs.(23)  and  (24)  it  then  follows  that  the  correct  choice  of  the  variables  a  and  b 
would  be 

Ax 

b  =  2(mr  -a,),  o=- : ,    a,  /  ±r»7r,  n  =  0,l,2,--  (29) 

2r  sin  a  j 

where,  b  will  be  chosen  such  that  it  is  a  positive  number,  and  the  absolute  value  of  a  will 
be  used  to  construct  the  trajectory.  The  direction  of  travel  along  the  closed  trajectory 
should  be  along  the  positive  or  the  negative  direction  depending  upon  whether  the  sign  of 
a  comes  out  to  be  positive  or  negative  in  Eq.(29). 

From  the  two  alternatives  we  conclude  that  if  the  final  value  of  a  is  such  that  af  = 
±(2n+  l)7r/2,  for  n  =  0,1,2,-  -,  then  we  will  first  converge  y  and  then  converge  x.  If 
aj  =  ±n7r,  for  n  =  0,1,2,- •-,  then  we  will  first  converge  x  and  then  converge  y.  For 
af  ^  nn/2,  for  n  =  0, 1, 2,  •  •  •,  either  of  the  two  alternatives  mentioned  above  can  be  adopted. 

To  illustrate  the  efficacy  of  this  singularity  free  algorithm,  we  consider  the  same  ex- 
ample we  have  considered  before.  The  initial  configuration  of  the  disk  of  radius  r  =  0.25 
metres  is  (x,,7/,,0,,a,)  =  (0.0,0.0,0.0,0.0)  metres,  degrees,  and  its  desired  configuration  is 
(xf,yf,6f,af)  =  (-0.4,1.0,180.0,22.5)  metres,  degrees.  We  first  converge  the  independent 
variables  using  a  straight  line  path  (path  segment  OP  in  Figs.  12  and   13)  in  the  0-a 
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plane.  At  the  end  of  this  path  the  configuration  of  the  system  is  found  to  be  (x,y,6,a)  = 
(0.1522,0.7654, 180.0,22.5)  metres  and  degrees  respectively.  Since  af  =  22.5  degrees  ^  rm/2, 
for  n  =  0, 1,2,  •  •  -,  therefore  we  can  choose  to  first  converge  x  or  y.  We  choose  to  converge  x 
first.  In  Eq.(23),  we  substitute  x/  =  -0.4  metres,  xd  =  0.1522  metres,  and  af  =  22.5  degrees. 
Choosing  b  -  60  degrees  or  1.047  radians,  we  obtain  a  =  3.628  radians  or  207.87  degrees.  For 
these  values  of  a  and  b,  the  closed  path  is  given  by  PQRSP  in  Fig.  12.  After  moving  along 
this  path,  the  y  variable  drifts  from  yd  —  0.7654  metres  to  y'd  —  1.485  metres,  as  shown  in 
Fig.  13,  whereas  the  x  coordinate  converges  to  its  desired  value.  Now  the  task  is  to  generate 
a  closed  path  in  the  6-a  plane  that  will  cause  a  repeatable  motion  in  x  but  will  converge  y 
to  yf  =  1.0  metre.  Using  Eq.(28)  we  arrive  at  b  =  135  degrees,  or  2.356  radians  and  a  =  -1.05 
radians,  or  60.15  degrees.  For  these  particular  values  of  a  and  £>,  the  closed  trajectory  is 
given  by  PXYZP  in  Fig.  12.  Due  to  the  negative  value  of  a  the  direction  of  this  closed 
path  is  opposite  to  our  usual  convention.  The  entire  motion  of  the  system  can  be  obtained 
by  eliminating  the  redundant  path  segment  SPS.  Therefore  the  complete  motion  of  the 
system  would  be  OPQRSXYZP,  as  shown  in  both  Figs.  12  and  13. 

5.   Conclusion 

A  motion  planning  algorithm  for  nonholonomic  mechanical  systems  was  presented  in 
this  paper.  In  this  algorithm,  the  independent  variables  of  the  system  were  first  converged 
to  their  desired  values.  Subsequently,  the  dependent  variables  were  converged  using  closed 
trajectories  of  the  independent  variables.  The  task  of  motion  planning  was  simplified  using 
Stokes's  theorem.  This  reduced  the  task  of  finding  closed  trajectories  of  the  independent 
variables  into  that  of  finding  a  surface  area  in  the  space  of  the  independent  variables  such 
that  the  dependent  variables  converged  to  their  desired  values  while  the  independent  vari- 
ables traversed  along  the  boundary  of  this  surface  area.  The  motion  planning  algorithm 
was  found  to  have  certain  global  attributes  due  to  which  questions  pertaining  to  the  reach- 
ability of  the  system  could  be  easily  answered  and  the  motion  could  be  planned  amidst 
additional  constraints.  The  salient  features  of  the  algorithm  was  aptly  illustrated  through 
the  examples  of  a  planar  space  robot  and  a  disk  rolling  without  slipping  on  a  flat  surface. 
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Figure  1.  A  disk  rolling  on  a  flat  surface  is  described  by  four  configuration 
variables:  x,  y,  0,  a.  The  disk  has  however  two  degrees  of  freedom  due  to 
the  presence  of  two  nonholonomic  constraints. 
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Figure  2  (a).  Positive  direction  of  travel  along  the  closed  curve  C  in  Stokes  Theorem 
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Figure  2  (b).  Positive  direction  of  travel  along  the  closed  curve  C  in  Green's  Theorem 
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Figure  3  (a).  The  closed  trajectory  C  in  the  independent  variables  x,  y,  and  z 
produces  a  change  in  the  dependent  variable  p  by  an  amount  Ap.  The  initial 
configuration  of  the  system  -  (1),  lies  on  this  closed  trajectory. 
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Figure  3  (b).  The  closed  trajectory  C  in  the  independent  variables  x,  y,  and  z 
produces  a  change  in  the  dependent  variable  p  by  an  amount  Ap.  The  initial 
configuration  of  the  system  -  (1),  does  not  lie  on  this  closed  trajectory. 
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Figure  4.  A  two-link  manipulator  mounted  on  a  space  vehicle  is  described 
by  three  generalized  coordinates:  0O,  8P  62.  The  center  of  mass  of  the 
space  vehicle  has  the  coordinates  x0,  y0. 
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Figure  5.  The  path  segment  OA  followed  by  the  closed  path  ABCDA  was 
used  to  converge  all  the  configuration  variables  of  the  space  robot,  for  the 
simulation  discussed  in  section  4.1.  The  closed  path  was  repeated  3  times. 
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Figure  6.  Evolution  of  all  the  configuration  variables  of  the  space  robot 
with  time,  for  the  simulation  discussed  in  section  4.1.  Points  O,  A,  B,  C 
and  D  in  this  figure  correspond  to  the  same  points  in  Fig. 5. 
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Figure  7.  Directed  paths  ABCD,  and  ADMN  both  change  the  orientation  of  the 
space  vehicle  by  equal  amounts.  The  closed  path  AXYZ  changes  the  orientation 
of  the  vehicle  in  four  cycles  by  the  same  amount  that  the  closed  paths  ABCD  and 
ADMN  can  bring  about  in  three  cycles. 
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Figure  8.  Both  the  closed  paths  PQRSP  and  PQMNP  in  the  6-a  plane 
produce  the  same  desired  changes  in  the  dependent  variables  x  and  y. 
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Figure  9.  A  diagrammatic  representation  of  the  singularity  avoidance  scheme 
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Figure  10.  The  path  OPQROZ  is  the  outcome  of  the  successful  implementation 
of  the  singularity  avoidance  scheme  for  the  rolling  disk,  discussed  in  section  4.2. 
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Figure  11.  Path  OPQROZ  shows  the  motion  of  the  disk  rolling  on  the 
x-y  plane,  for  the  simulation  discussed  in  section  4.2  on  the  singularity 
avoidance  scheme.  Fig.  10  shows  the  motion  of  the  disk  on  the  G-a  plane. 
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Figure  12.  The  path  OPQRSXYZP  was  planned  using  the  singularity 
free  algorithm  with  a  repeatable  motion  in  the  x  coordinate 
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Figure  13.  Path  OPQRSXYZP  shows  the  motion  of  the  disk  rolling  on  the 
x-y  plane.  This  motion  was  planned  using  the  singularity-free  algorithm, 
discussed  in  section  4.2,  with  a  repeatable  motion  in  the  x  coordinate.  The 
corresponding  motion  of  the  disk  in  the  9-a  planeisshowninFig.il. 
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